package robocup.component.situations;

import java.util.Arrays;
import java.util.EnumMap;

import robocup.component.Prophet;
import robocup.component.SConf;
import robocup.component.WorldModel;
import robocup.component.ControllerNumbers.PLAY_MODE;
import robocup.component.actions.Action;
import robocup.component.actions.ChangeViewModeAction;
import robocup.component.geometry.Vektor;
import robocup.component.tactics.AbstractState.STATES;
import robocup.component.worldobjects.Ball;
import robocup.component.worldobjects.DynamicObject;
import robocup.component.worldobjects.Player;
import robocup.component.worldobjects.Player.UPDATE_MODE;

/**
 * @author Thomas Karbe
 * 
 */
public class TurnNeckSituation
    extends Situation {

  /**
   * The statistics for calling actions(states).
   * 
   * @author rw
   */
  private class Statistics {

    // map containing booleans
    private final EnumMap<STATES, Integer> actionCounter = new EnumMap<STATES, Integer>(
                                                             STATES.class);

    private int                            counter       = 0;

    /**
     * creates new statistics
     */
    public Statistics() {

      this.reset();
    }

    private void reset() {

      for (final STATES a : STATES.values()) {
        this.actionCounter.put(a, new Integer(0));
      }
      this.counter = 0;
    }

    /**
     * @return the counter
     */
    public int getCounter() {

      return this.counter;
    }

    private Integer getCounter(final STATES a) {

      return this.actionCounter.get(a);
    }

    /**
     * increases the counter for s
     * 
     * @param s
     *          the state to count
     */
    public void update(final STATES s) {

      this.actionCounter.put(s, this.getCounter(s) + 1);
      this.counter++;
    }

    /**
     * prints the statistics on screen
     */
    public void print() {

      System.out.println("NeckRotatorStatistics:");
      for (final STATES a : STATES.values()) {
        System.out
            .println(a.toString() + " #:" + this.getCounter(a) + " %:" + ((double) this
                .getCounter(a).intValue() / this.counter));
      }
    }

  }

  // neckRotator statistics
  private final Statistics statistics           = new Statistics();

  double[]                 preferredObjects     = new double[23];
  boolean                  setPref              = false;
  Player                   dummyPlayer          = Player.getDummyInstance();

  private final double[]   finalSlices          = new double[(int) Math
                                                    .pow(
                                                        2,
                                                        this
                                                            .getWorld()
                                                            .getNumberViewSlicesExp())];

  private final int[]      seeableSlices        = new int[(int) Math
                                                    .pow(
                                                        2,
                                                        this
                                                            .getWorld()
                                                            .getNumberViewSlicesExp())];

  private final double[]   stateSlices          = new double[(int) Math
                                                    .pow(
                                                        2,
                                                        this
                                                            .getWorld()
                                                            .getNumberViewSlicesExp())];

  private int              searchCount          = 0;

  private STATES           thisState            = null;

  Ball                     dummyBall            = Ball.getDummyInstance();

  Vektor                   objectPos            = new Vektor();

  private final double[]   importantObjectValue = new double[(int) Math
                                                    .pow(
                                                        2,
                                                        this
                                                            .getWorld()
                                                            .getNumberViewSlicesExp())];

  private STATES           lastState            = null;

  Vektor                   dummyVektor1         = Vektor.getDummyInstance();

  Vektor                   dummyVektor2         = Vektor.getDummyInstance();

  private double           stateMax             = 0.;

  Vektor                   oppLeftPost          = new Vektor(
                                                    Vektor.XY,
                                                    SConf.getInstance().half_goal_width,
                                                    SConf.getInstance().half_length);

  Vektor                   oppRightPost         = new Vektor(
                                                    Vektor.XY,
                                                    -SConf.getInstance().half_goal_width,
                                                    SConf.getInstance().half_length);

  private final double     areaWeight           = 0.5;

  private final double     historyWeight        = 0.5;

  private final double     stateWeight          = 3.0;

  /**
   * Creates a new TurnNeckSituation
   * 
   * @param world
   *          the WorldModel
   * @param prophet
   *          the prophet
   * @param movements
   *          The Movements
   */
  public TurnNeckSituation(
      final WorldModel world,
      final Prophet prophet,
      final Movements movements) {

    super(world, prophet, movements);

    // state values aren't already set
    Arrays.fill(this.seeableSlices, 1);
    // state values aren't already set
    Arrays.fill(this.stateSlices, 1);
    // final values will be calculated later
    Arrays.fill(this.finalSlices, 1);
    // initially there are no important objects
    Arrays.fill(this.importantObjectValue, 0);
  }

  /**
   * The best view direction (abs).
   * 
   * @param bodyDir -
   *          players body direction
   * @param viewWidth -
   *          players view width
   * @param state -
   *          calling state
   * @return - best head angle (abs)
   */
  private double bestViewDirAbs(final double bodyDir,
      final double viewWidth,
      final STATES state) {

    Arrays.fill(this.preferredObjects, 0);
    this.setPref = false;
    return this.calcBestViewDir(bodyDir, viewWidth, state);
  }

  /**
   * The best view direction (abs).
   * 
   * @param bodyDir -
   *          players body direction
   * @param viewWidth -
   *          players view width
   * @param state -
   *          calling state
   * @param pref
   *          an size 23 array with values from -1 (not look at object) over 0
   *          (look normal at object) to 1 (always look at object) place 0-10
   *          are own players, place 11 - 21 are opponent players, place 22 is
   *          the ball
   * @return - best head angle (abs)
   */
  private double bestViewDirAbs(final double bodyDir,
      final double viewWidth,
      final STATES state,
      final double[] pref) {

    this.preferredObjects = pref;
    this.setPref = true;
    return this.calcBestViewDir(bodyDir, viewWidth, state);
  }

  /**
   * The best view direction (abs).
   * 
   * @param bodyDir -
   *          players body direction
   * @param viewWidth -
   *          players view width
   * @param state -
   *          calling state
   * @param pref
   *          the object to look at always
   * @return - best head angle (abs)
   */
  private double bestViewDirAbs(final double bodyDir,
      final double viewWidth,
      final STATES state,
      final DynamicObject pref) {

    Arrays.fill(this.preferredObjects, 0);
    if (pref instanceof Ball) {
      this.preferredObjects[22] = 1;
    }
    else {
      this.dummyPlayer.copy(((Player) pref));
      if (this.dummyPlayer.isFriend()) {
        this.preferredObjects[this.dummyPlayer.getNumber() - 1] = 1;
      }
      else {
        this.preferredObjects[this.dummyPlayer.getNumber() + 10] = 1;
      }
    }
    this.setPref = true;
    return this.calcBestViewDir(bodyDir, viewWidth, state);
  }

  /**
   * show the state statistic.
   * 
   */
  public void showStatistics() {

    this.statistics.print();
  }

  /**
   * For debugging. Shows the NR-values depending on the state.
   * 
   * @param state -
   *          state
   * @param slice
   *          the slice
   */
  public void showValues(final STATES state,
      final int slice) {

    System.out.println("Slice to view: " + slice);
    System.out.println("Cycle: " + this.getWorld().getCycle());
    switch (state) {
      default:
        System.out.println("   actual State " + state.toString());
        System.out.println("Slices: History " + Arrays.toString(this.getWorld()
            .getViewSliceHistory()));
        System.out.println("        Area    " + Arrays.toString(this.getWorld()
            .getEstimatedViewSliceAreas()));
        System.out.println("        Seeable " + Arrays
            .toString(this.seeableSlices));
        System.out.println("        State   " + Arrays
            .toString(this.stateSlices));
        System.out.println("        Final   " + Arrays
            .toString(this.finalSlices));
        System.out.println();
    }
  }

  private double calcBestViewDir(final double bodyDir,
      final double viewWidth,
      final STATES state) {

    this.statistics.update(state);
    this.thisState = state;
    if (this.thisState != STATES.SEARCH_BALL) {
      this.searchCount = 0;
    }
    this.setUnseeableSlices(bodyDir, viewWidth);
    if (this.setPref) {
      for (int i = 0; i < this.preferredObjects.length; i++) {
        if (this.preferredObjects[i] != 0) {
          if (i < 22) {
            this.dummyPlayer.copy(this.getWorld().getExpectedPlayers()[i]);
            this.dummyPlayer.extrapolate(1);
            this.objectPos.copy(this.dummyPlayer.getPosition());
          }
          else {
            // the ball
            this.dummyBall.copy(this.getWorld().getBallRef());
            this.dummyBall.extrapolate(1);
            this.objectPos.copy(this.dummyBall.getPosition());
          }
          final int objectSlice = this.getWorld().getViewSliceContainingPoint(
              this.getWorld().getMyNextPosition(), this.objectPos);
          if (this.seeableSlices[objectSlice] == 0) {
            this.preferredObjects[i] = 0;
          }
        }
      }
    }
    // now preferred contains only seeable dynamic objects
    // set important object values

    boolean found = false;
    for (final double pref : this.preferredObjects) {
      if (pref != 0) {
        found = true;
      }
    }
    this.setPref = found;

    if (!this.setPref) {
      Arrays.fill(this.importantObjectValue, 0);
    }
    else {
      Arrays.fill(this.importantObjectValue, -1);
      for (int i = 0; i < this.preferredObjects.length; i++) {
        if (this.preferredObjects[i] != 0) {
          if (i < 22) {
            this.dummyPlayer.copy(this.getWorld().getExpectedPlayers()[i]);
            this.dummyPlayer.extrapolate(1);
            this.objectPos.copy(this.dummyPlayer.getPosition());
          }
          else {
            // the ball
            this.dummyBall.copy(this.getWorld().getBallRef());
            this.dummyBall.extrapolate(1);
            this.objectPos.copy(this.dummyBall.getPosition());
          }
          final int objectSlice = this.getWorld().getViewSliceContainingPoint(
              this.getWorld().getMyNextPosition(), this.objectPos);
          final double objectDist = this.getWorld().getMyNextPosition()
              .getDistance(this.objectPos);
          int importantSliceNum = 0;
          if (objectDist < 5) {
            importantSliceNum = 6;
          }
          else if (objectDist < 6) {
            importantSliceNum = 5;
          }
          else if (objectDist < 7) {
            importantSliceNum = 4;
          }
          else if (objectDist < 8) {
            importantSliceNum = 3;
          }
          else if (objectDist < 9) {
            importantSliceNum = 2;
          }
          else {
            importantSliceNum = 1;
          }

          final int maxSliceDiff = (int) ((viewWidth / this.getWorld()
              .getViewSliceWidth()) / 2);
          if (maxSliceDiff < 1) {
            System.out.println("Error in NR --> no slices seeable???");
          }

          for (int j = 0; j < this.getWorld().getNumberViewSlices(); j++) {
            final int sliceDiff = this.getWorld().getViewSliceDiff(j,
                objectSlice);
            double value = 0;
            if (sliceDiff > maxSliceDiff) {
              value = -1;
            }
            else if (sliceDiff > importantSliceNum) {
              value = this.preferredObjects[i];
            }
            else {
              value = this.preferredObjects[i] - (sliceDiff / importantSliceNum);
            }
            this.importantObjectValue[j] = Math.max(value,
                this.importantObjectValue[j]);
          }
        }
      }
    }

    if (state == STATES.PASS) {
      // watch ball and close players
      this.setDefaultValues(this.getWorld().getMyPosition(), 4, 0.1, 1, 0.5, 0,
          0, false, true, 2, 1, 1);
    }
    else if (state == STATES.DRIBBLE) {
      this.setDefaultValues(this.getWorld().getMyPosition(), 0, 4, 4, 1, 0, 0,
          false, false, 4, 1, this.getWorld().getMyPosition().y > 30 ? 4 : 1);
      // setDribbleValues(world.getMyPosition());
    }
    else if (state == STATES.GOALKEEPER) {
      // watch only ball
      this.setDefaultValues(this.getWorld().getMyPosition(), 0, 1, 1, 1, 1, 1,
          true, true, 1, 1, 1);
    }
    else if (state == STATES.GOALIE_KICK_IN) {
      // watch much field
      this.setDefaultValues(this.getWorld().getMyPosition(), 0, 0.1, 5, 5, 1,
          0, false, false, 4, 1, 0);
    }
    else if (state == STATES.SHOOT_GOAL) {
      // watch much field
      this.setDefaultValues(this.getWorld().getMyPosition(), 0, 0.1, 0.1, 0.1, 0.1,
          0, false, false, 0.1, 1, 10);
    }
    else if (state == STATES.INTERCEPT_BALL) {
      // watch primary to ball
      if ((this.getWorld().getMyPosition().getDistance(
          SConf.getInstance().GOAL_POS_OTHER) < 20) && (this.prophet
          .getInterceptionTime() == 1) && (this.getWorld().getBallRef().getAge(
          this.getWorld().getBodyCycle(),
          robocup.component.worldobjects.Ball.UPDATE_MODE.SEEN_WITHOUT_SPEED) < 4)) {
        // System.out.println(getWorld().getCycleAndPlayerString()+" watch
        // goalie before interception");
        this.setDefaultValues(this.getWorld().getMyPosition(), 0.1, 0.1, 0.1, 0.1, 0,
            0, false, false, 0.1, 2, 10);
      }
      else if ((this.prophet.getInterceptionTime() <= 3) && (this
          .getWorld()
          .getBallRef()
          .getAge(
              this.getWorld().getBodyCycle(),
              robocup.component.worldobjects.Ball.UPDATE_MODE.SEEN_WITHOUT_SPEED) < 4) && ((this
          .getWorld().getOpponentsGoalie() == null) || (this.getWorld()
          .getOpponentsGoalie().getAge(this.getWorld().getBodyCycle(),
              UPDATE_MODE.SEEN_TEAM_OTHER) > 3))) {
        this.setDefaultValues(this.getWorld().getMyPosition(), 1, 0.1, 3, 1, 0,
            0, false, true, 1, 2, 10);
      }
      else {
        this.setDefaultValues(this.getWorld().getMyPosition(), 100, 0.1, 3, 1,
            0, 0, false, true, 1, 2, 1);
      }
    }
    else if (state == STATES.SEARCH_BALL) {
      this.searchCount++;
      if (!(this.lastState == STATES.SEARCH_BALL)) {
        this.setSearchBallValues(this.getWorld().getMyPosition());
      }
    }
    else if (state == STATES.START_POSITION) {
      // watch ball
      // and players
      // close
      // players are
      // more
      // important
      // then far
      // away
      this.setDefaultValues(this.getWorld().getMyPosition(), 4, 0.1, 2, 0.75,
          0.25, 0.1, false, true, 1, 1, 0.5);
    }
    else if (state == STATES.BACKFOUR_POSITIONING_NEAR_MIDLINE) {
      this.setDefaultValues(this.getWorld().getMyPosition(), 4, 2, 2, 0.75,
          0.25, 0.05, false, true, 1, 1, 0.5);
    }
    else {
      // watch
      // ball
      // and
      // players
      // close
      // players
      // are
      // more
      // important
      // then
      // far
      // away
      this.setDefaultValues(this.getWorld().getMyPosition(), 4, 0.1, 2, 0.75,
          0.25, 0.1, false, true, 1, 1, 1);
    }

    this.lastState = state;

    // number of relevant slices (slices that can be seen with given view
    // width)
    int slices = (int) (viewWidth / this.getWorld().getViewSliceWidth());
    // at least one slice
    if (slices == 0) {
      slices++;
    }

    // init values

    // max Value for covered slices
    double maxValue = -1;
    double tempValue = 0;
    // middle of a slice
    double bestViewDirAbs = 0;
    double viewDirAbs = 0;
    final int numberOfSlices = this.getWorld().getNumberViewSlices();
    // get the sequence of slices with the max. sum value
    for (int i = 0; i < numberOfSlices; i++) {
      tempValue = 0;
      for (int j = 0; j < slices; j++) {
        tempValue += this.getFinalValue((i + j) % numberOfSlices, state);
      }
      if (tempValue > maxValue) {
        viewDirAbs = Vektor
            .normalize(this.getWorld().getViewSliceLeftAngle(i) + slices * this
                .getWorld().getViewSliceWidth() / 2);
        if (Math.abs(Vektor.normalize(viewDirAbs - bodyDir)) <= 90) {
          maxValue = tempValue;
          bestViewDirAbs = viewDirAbs;
        }
      }
    }
    for (int i = 0; i < numberOfSlices; i++) {
      this.finalSlices[i] = this.getFinalValue(i, state);
      this.finalSlices[i] = Math.round(this.finalSlices[i] * 1000) / 1000.;
    }
    if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
      this.showValues(state, this.getWorld().getViewSliceContainingAbsAngle(
          bestViewDirAbs));
      System.out.print("\nslice to view: ");
      System.out.println(this.getWorld().getViewSliceContainingAbsAngle(
          bestViewDirAbs));
    }

    return bestViewDirAbs;
  }

  /**
   * Slices in the back of the player that can't be seen are set to 0.
   * 
   * @param bodyDir -
   *          abs body dir angle
   * @param viewWidth -
   *          view cone width
   */
  private void setUnseeableSlices(final double bodyDir,
      final double viewWidth) {

    // margins for seeing something
    final double leftMax = Vektor
        .normalize(bodyDir + SConf.getInstance().MINNECKANG - viewWidth / 2);
    final double rightMax = Vektor
        .normalize(bodyDir + SConf.getInstance().MAXNECKANG + viewWidth / 2);
    // margins skip the +-180 border (sign swaps)
    boolean skips180 = leftMax > rightMax;

    // number of relevant slices (slices that can be seen with given view
    // width)
    int slices = (int) (viewWidth / this.getWorld().getViewSliceWidth());

    // at least one slice
    if (slices == 0) {
      System.out
          .println("0 Slices can be seen --> impossible, correct Neckrotator");
      slices++;
    }

    // middle of a slice
    double meridian = 0;

    // set unseeable to 0
    for (int i = 0; i < this.getWorld().getNumberViewSlices(); i++) {
      meridian = this.getWorld().getViewSliceMeridian(i);
      // no skipping
      if (!skips180 && (meridian >= leftMax) && (meridian <= rightMax)) {
        this.seeableSlices[i] = 1;
      }
      // skipping
      else if (skips180 && ((meridian > leftMax) || (meridian < rightMax))) {
        this.seeableSlices[i] = 1;
      }
      // not seen
      else {
        this.seeableSlices[i] = 0;
      }
      if ((Math.abs(SConf.getInstance().MINNECKANG - viewWidth / 2) + SConf
          .getInstance().MAXNECKANG + viewWidth / 2) >= 360) {
        this.seeableSlices[i] = 1;
      }
    }
  }

  /**
   * sets the slice-values depending on several weights and depending on the
   * objects within this slice
   * 
   * @param pos -
   *          position of the player
   * @param ballWeight -
   *          importance of the ball
   * @param weight3m -
   *          importance of other players who are within 3m around the player
   *          should be a small value because these players are already sensed
   *          with body
   * @param weight10m -
   *          importance of other players who are within 3 - 10m around the
   *          player
   * @param weight25m -
   *          importance of other players who are within 10 - 25m around the
   *          player
   * @param weight40m -
   *          importance of other players who are within 25 - 40m around the
   *          player
   * @param farAwayWeight -
   *          importance of other players who are more than 40m away from the
   *          player
   * @param kickableTeam -
   *          ball is more important, if it can be kicked by a teammate?
   * @param kickableOpp -
   *          ball is more important, if it can be kicked by an opponent?
   * @param teamWeight -
   *          how important are teamMates?
   * @param oppWeight -
   *          how important are opponent players?
   * @param oppGoalieWeight -
   *          maybe I should look to the opponent goalie sometimes?
   * 
   */

  private void setDefaultValues(final Vektor pos,
      double ballWeight,
      final double weight3m,
      final double weight10m,
      final double weight25m,
      final double weight40m,
      final double farAwayWeight,
      final boolean kickableTeam,
      final boolean kickableOpp,
      final double teamWeight,
      final double oppWeight,
      final double oppGoalieWeight) {

    double dist2player;
    double dist2Ball;
    double dist2Goal;
    double posWeight = 1;
    double roleWeight = 1;
    final Vektor ballPos = this.getWorld().getNextPosition(
        this.getWorld().getBallRef());
    boolean kickable = false;
    final Player[] players = this.getWorld().getExpectedPlayers();// (rw).getAllPlayers();

    // init slices
    for (int i = 0; i < this.stateSlices.length; i++) {
      this.stateSlices[i] = 0.1;
    }

    // set ball-values
    final int ballSlice = this.getWorld().getViewSliceContainingPoint(pos,
        ballPos);
    int leftSlice = this.getWorld().getLeftViewSliceNeighbour(ballSlice);
    int rightSlice = this.getWorld().getRightViewSliceNeighbour(ballSlice);
    double ballAge = this.getWorld().getBallRef().getAge(
        this.getWorld().getBodyCycle(),
        robocup.component.worldobjects.Ball.UPDATE_MODE.SEEN_WITHOUT_SPEED) + 0.1;
    ballAge = Math.min(this.getWorld().getBallRef().getAge(
        this.getWorld().getBodyCycle(),
        robocup.component.worldobjects.Ball.UPDATE_MODE.HEARED) + 1, ballAge);

    if (this.preferredObjects[22] < 0) {
      ballWeight *= (1 + this.preferredObjects[22]);
    }
    this.stateMax = 0;
    if (!(this.getWorld().getPlayMode() == PLAY_MODE.PLAY_ON) && this
        .getWorld().getBallRef().isCatched() && (this.getWorld()
        .getOpponentsGoalie() != null)) {
      if (this.getWorld().getOpponentsGoalie().getDistance(
          this.getWorld().getBallRef()) < 10) {
        this.dummyVektor1.x = -SConf.getInstance().half_penalty_width;
        this.dummyVektor1.y = SConf.getInstance().half_length - SConf
            .getInstance().penalty_length;
        this.dummyVektor2.x = SConf.getInstance().half_penalty_width;
        this.dummyVektor2.y = SConf.getInstance().half_length - SConf
            .getInstance().penalty_length;
        final int startslice = this.getWorld().getViewSliceContainingPoint(pos,
            this.dummyVektor1);
        final int endslice = this.getWorld().getViewSliceContainingPoint(pos,
            this.dummyVektor2);
        // System.out.println("Opponent Goalie catched Ball: NR looks
        // slices "+startslice+" to "+endslice);
        for (int i = startslice; i <= endslice; i++) {
          this.stateSlices[i] += ballAge * ballWeight;
          if (this.stateMax < this.stateSlices[i]) {
            this.stateMax = this.stateSlices[i];
          }
        }
      }
    }
    else {
      this.stateSlices[ballSlice] += ballAge * ballWeight;
      this.stateSlices[leftSlice] += ballAge * ballWeight / 2;
      this.stateSlices[rightSlice] += ballAge * ballWeight / 2;
      if (this.stateMax < this.stateSlices[ballSlice]) {
        this.stateMax = this.stateSlices[ballSlice];
      }
    }
    // set opponent goal values
    final int goalSlice = this.getWorld().getViewSliceContainingPoint(pos,
        SConf.getInstance().GOAL_POS_OTHER);
    leftSlice = this.getWorld().getLeftViewSliceNeighbour(goalSlice);
    rightSlice = this.getWorld().getRightViewSliceNeighbour(goalSlice);
    dist2Goal = pos.getDistance(SConf.getInstance().GOAL_POS_OTHER);
    if (dist2Goal < 20) {
      this.stateSlices[goalSlice] += 10;
      this.stateSlices[leftSlice] += 6;
      this.stateSlices[rightSlice] += 6;
    }
    if (this.stateMax < this.stateSlices[goalSlice]) {
      this.stateMax = this.stateSlices[goalSlice];
    }
    if (this.stateMax < this.stateSlices[leftSlice]) {
      this.stateMax = this.stateSlices[leftSlice];
    }
    if (this.stateMax < this.stateSlices[rightSlice]) {
      this.stateMax = this.stateSlices[rightSlice];
    }

    for (int i = 0; i < players.length; i++) {

      // skip the self
      if (i == this.getWorld().getNumber() - 1) {
        continue;
      }

      double vAge;
      if (i < 11) {
        vAge = players[i].getAge(this.getWorld().getBodyCycle(),
            UPDATE_MODE.SEEN_NOTEAM) + 0.1;
      }
      else {
        vAge = players[i].getAge(this.getWorld().getBodyCycle(),
            UPDATE_MODE.META_BASED_ON_INFO) + 0.1;
      }

      final Vektor ppos = players[i].getPosition();
      dist2player = pos.getDistance(ppos);
      dist2Ball = ballPos.getDistance(ppos);
      if (!this.getWorld().inField(ppos)) {
        posWeight = 0.0;
      }
      else if (players[i].isMe()) {
        posWeight = 0.0;
      }
      else if (dist2player < 3) {
        posWeight = weight3m;
      }
      else if (dist2player < 10) {
        posWeight = weight10m;
      }
      else if (dist2player < 25) {
        posWeight = weight25m;
      }
      else if (dist2player < 40) {
        posWeight = weight40m;
      }
      else {
        posWeight = farAwayWeight;
      }
      if ((ppos.y > this.getWorld().getMyPosition().y) && players[i].isFriend()) {
        posWeight *= 4;
      }
      else if ((ppos.y > this.getWorld().getMyPosition().y - 10) && players[i]
          .isFriend()) {
        posWeight *= 2;
      }
      if (this.preferredObjects[i] < 0) {
        posWeight *= (this.preferredObjects[i] + 1);
      }
      final int pSlice = this.getWorld().getViewSliceContainingPoint(pos, ppos);
      leftSlice = this.getWorld().getLeftViewSliceNeighbour(pSlice);
      rightSlice = this.getWorld().getRightViewSliceNeighbour(pSlice);
      if (players[i].isFriend()) {
        roleWeight = teamWeight;
        if ((players[i].getKickDistance() >= dist2Ball) && kickableTeam) {
          kickable = true;
        }
      }
      else {
        if ((players[i].getKickDistance() >= dist2Ball) && kickableOpp) {
          kickable = true;
        }
        if (players[i].isGoalie()) {
          roleWeight = oppGoalieWeight;
        }
        else {
          roleWeight = oppWeight;
        }
      }
      if (pSlice >= 0) {
        if (players[i].isReliable()) {
          this.stateSlices[pSlice] += (vAge * posWeight * roleWeight);
          this.stateSlices[leftSlice] += (vAge * posWeight * roleWeight / 2.);
          this.stateSlices[rightSlice] += (vAge * posWeight * roleWeight / 2.);
        }
        else {
          if (players[i].isFriend() || !(players[i].isGoalie())) {
            this.stateSlices[pSlice] += vAge * posWeight * roleWeight / 3.;
            this.stateSlices[leftSlice] += (vAge * posWeight * roleWeight / 6.);
            this.stateSlices[rightSlice] += (vAge * posWeight * roleWeight / 6.);
          }
          else {
            // it's the opponent goalie
            final int lGoalSlice = this.getWorld().getViewSliceContainingPoint(
                pos, this.oppLeftPost);
            final int rGoalSlice = this.getWorld().getViewSliceContainingPoint(
                pos, this.oppRightPost);
            final int mGoalSlice = this.getWorld().getViewSliceContainingPoint(
                pos, SConf.getInstance().GOAL_POS_OTHER);
            this.stateSlices[lGoalSlice] += (vAge * posWeight * roleWeight);
            this.stateSlices[rGoalSlice] += (vAge * posWeight * roleWeight);
            this.stateSlices[mGoalSlice] += (vAge * posWeight * roleWeight);
          }
        }
        if (this.stateMax < this.stateSlices[pSlice]) {
          this.stateMax = this.stateSlices[pSlice];
        }
        if (this.stateMax < this.stateSlices[leftSlice]) {
          this.stateMax = this.stateSlices[pSlice];
        }
        if (this.stateMax < this.stateSlices[rightSlice]) {
          this.stateMax = this.stateSlices[pSlice];
        }
      }
    }
    if (kickable && (this.preferredObjects[22] > 0)) {
      this.stateSlices[ballSlice] = 10 * this.stateMax;
      leftSlice = this.getWorld().getLeftViewSliceNeighbour(ballSlice);
      rightSlice = this.getWorld().getRightViewSliceNeighbour(ballSlice);
      this.stateSlices[leftSlice] += this.stateMax;
      this.stateSlices[rightSlice] += this.stateMax;
      this.stateMax *= 10;
    }
    for (int i = 0; i < this.stateSlices.length; i++) {
      this.stateSlices[i] /= this.stateMax;
      this.stateSlices[i] *= 10;
      if (this.stateSlices[i] < 0.1) {
        this.stateSlices[i] = 0.1;
      }
    }
  }

  /**
   * Sets a value for each slice that represents the probability that the slice
   * contains the ball.
   * 
   * @param pos
   *          the players position
   */
  private void setSearchBallValues(final Vektor pos) {

    this.searchCount = 1;
    final int ballSlice = this.getWorld().getViewSliceContainingPoint(pos,
        this.getWorld().getNextPosition(this.getWorld().getBallRef()));
    double diff;
    // the ball prob of each slice depends of the slice's distance to the
    // slice
    // of the last ball
    for (int i = 0; i < this.stateSlices.length; i++) {
      diff = Math.abs(i - ballSlice);
      if (diff > this.stateSlices.length / 2) {
        diff = this.stateSlices.length - diff;
      }
      // exp. relation between distance and value
      this.stateSlices[i] = 10 / Math.pow(2, diff / 2);
      if (this.stateSlices[i] < 0.01) {
        this.stateSlices[i] = 0.01;
      }

    }
  }

  /**
   * The value of a slice.
   * 
   * @param i -
   *          slice index
   * @param state -
   *          the state calling
   * @return value of slice i
   * 
   */
  private double getFinalValue(final int i,
      final STATES state) {

    if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
      System.out.println("historyWeight: " + this.historyWeight);
      System.out.println("stateWeight: " + this.stateWeight);
      System.out.println("areaWeight: " + this.areaWeight);
      System.out.println("areaMax: " + this.getWorld().getViewSliceAreaMax());
      System.out
          .println("important[" + i + "]: " + this.importantObjectValue[i]);
    }

    switch (state) {
      case SEARCH_BALL: {
        final double value = (Math.pow(this.roundHistory(this.getWorld()
            .getViewSliceHistory()[i]), this.historyWeight) * Math.pow(
            this.stateSlices[i], this.stateWeight) * Math.pow(this.getWorld()
            .getEstimatedViewSliceAreas()[i] / (this.getWorld()
            .getViewSliceAreaMax() / 10), this.areaWeight));
        if (this.importantObjectValue[i] >= 0) {
          return (this.importantObjectValue[i] + (1 - this.importantObjectValue[i]) * value);
        }
        return ((1 + this.importantObjectValue[i]) * value);
      }
      case DRIBBLE:
      default: {
        final double value = (Math.pow(this.roundHistory(this.getWorld()
            .getViewSliceHistory()[i]), this.historyWeight) * Math.pow(
            this.stateSlices[i], this.stateWeight) * Math.pow(this.getWorld()
            .getEstimatedViewSliceAreas()[i] / (this.getWorld()
            .getViewSliceAreaMax() / 10), this.areaWeight));
        if (this.importantObjectValue[i] >= 0) {
          return ((this.importantObjectValue[i] + (1 - this.importantObjectValue[i]) * value) * this.seeableSlices[i]);
        }
        return ((1 + this.importantObjectValue[i]) * value * this.seeableSlices[i]);
      }
    }

  }

  /**
   * 
   * @param hist -
   *          a history value
   * @return an value, that stands for the importance of the goven history
   */
  private double roundHistory(final double hist) {

    if (hist < 1) {
      return 0.1;
    }
    if (hist < 2) {
      return 1.5;
    }
    if (hist < 3) {
      return 2.5;
    }
    if (hist < 4) {
      return 3.5;
    }
    if (hist < 5) {
      return 4.0;
    }
    if (hist < 6) {
      return 4.5;
    }
    if (hist < 8) {
      return 5.;
    }
    if (hist < 10) {
      return 6.;
    }
    if (hist < 25) {
      return 7.;
    }
    if (hist < 50) {
      return 8.;
    }
    if (hist < 100) {
      return 9.;
    }
    return 10.;

  }

  /**
   * (non-Javadoc)
   * 
   * @see robocup.component.situations.Situation#update()
   */
  @Override
  public void update() {

    super.update();
    this.setPref = false;

    if (this.getWorld().isViBased()) {

      final double leftBorder = Vektor.normalize(this.getWorld().getSelfRef()
          .getHeadDir() - this.getWorld().getViewWidth() / 2.0);
      final double rightBorder = Vektor.normalize(this.getWorld().getSelfRef()
          .getHeadDir() + this.getWorld().getViewWidth() / 2.0);
      double overlapPercentage;
      // history maximum will be calculated new
      for (int i = 0; i < this.getWorld().getViewSliceHistory().length; i++) {
        overlapPercentage = this.getWorld().overlapAreaPercentage(i,
            leftBorder, rightBorder);
        // != 1.0 because every slice should be more interesting than
        // "unseeable" slices (value 0)
        if ((this.thisState == STATES.SEARCH_BALL) && (overlapPercentage >= 0.95)) {
          this.stateSlices[i] *= 0.1;
        }
        else if (this.thisState == STATES.SEARCH_BALL) {
          this.stateSlices[i] += 1;
        }
      }
    }

  }

  boolean debugTN = false;

  /**
   * The best angle to turn the neck.
   * 
   * @param firstAction -
   *          the choosen main action
   * @param viewModeAction -
   *          the choosen view mode action
   * @param a -
   *          calling action (state)
   * 
   * @return angle to turn neck (rel to headAngle)
   * 
   */
  public double getBestTurnNeckAngle(final Action firstAction,
      final ChangeViewModeAction viewModeAction,
      final STATES a) {

    if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
      final StringBuffer sb = new StringBuffer();
      sb.append("C: ").append(this.getWorld().getCycle());
      sb.append(" BS: ").append(this.getWorld().getBodyCycle());
      sb.append("\ncall Prophet.getBTNAng( ").append(firstAction);
      sb.append(", ").append(viewModeAction);
      sb.append(", ").append(a).append(")\n");
      System.out.println(sb);
    }

    if (!this.wasUpdated()) {
      this.update();
      if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
        System.out.println("needed Update");
      }
    }

    final double viewWidth = (viewModeAction == null) ? this.getWorld()
        .getViewWidth() : viewModeAction.getViewWidth();
    final Player p = this.getWorld().getSelfRef().cloned();
    p.predictThisAfterAction(firstAction);
    // best view angle

    if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
      System.out.print("call bestViewDirAbs(");
      System.out.print(p.getBodyDir());
      System.out.print(", ");
      System.out.print(viewWidth);
      System.out.print(", ");
      System.out.print(a);
      System.out.println(")");
    }
    final double angleAbs = this.bestViewDirAbs(p.getBodyDir(), viewWidth, a);

    if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
      System.out.print("resulting angle: ");
      System.out.println(angleAbs);
    }

    // some corrections....
    double angleToBody = Vektor.normalize(angleAbs - p.getBodyDir());

    // this should never happen
    if ((angleToBody < SConf.getInstance().MINNECKANG - this.getWorld()
        .getViewSliceWidth()) || (angleToBody > SConf.getInstance().MAXNECKANG + this
        .getWorld().getViewSliceWidth())) {
      System.out
          .println("NeckRotator breaks neck: C" + this.getWorld().getCycle() + " P" + this
              .getWorld().getNumber() + "State:" + a + " result. Neckangle=" + angleToBody + " abs.Angle=" + angleAbs + " BD:" + p
              .getBodyDir() + " ND:" + p.getNeckAngle());
      this.showValues(a, 0);
    }

    // as the NeckRotator is discret, it might sometimes return to high
    // values
    // max error is neckRotator.getSliceWidth() (11.5 degree by default)
    if (angleToBody < SConf.getInstance().MINNECKANG) {
      // + 0.001 for avoiding 180� turn in wrong direction
      angleToBody = SConf.getInstance().MINNECKANG + 0.001;
    }
    if (angleToBody > SConf.getInstance().MAXNECKANG) {
      // - 0.001 for avoiding 180� turn in wrong direction
      angleToBody = SConf.getInstance().MAXNECKANG - 0.001;
    }

    if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
      System.out.print("resulting angle: ");
      System.out.println(Vektor.normalize(angleToBody - p.getNeckAngle()));
      System.out.println("==================================================");
    }
    return Vektor.normalize(angleToBody - p.getNeckAngle());
  }

  /**
   * The best angle to turn the neck.
   * 
   * @param firstAction -
   *          the choosen main action
   * @param viewModeAction -
   *          the choosen view mode action
   * @param a -
   *          calling action (state)
   * @param obj -
   *          preferred object to see
   * 
   * @return angle to turn neck (rel to headAngle)
   * 
   */
  public double getBestTurnNeckAngle(final Action firstAction,
      final ChangeViewModeAction viewModeAction,
      final STATES a,
      final DynamicObject obj) {

    if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
      final StringBuffer sb = new StringBuffer();
      sb.append("C: ").append(this.getWorld().getCycle());
      sb.append(" BS: ").append(this.getWorld().getBodyCycle());
      sb.append("\ncall Prophet.getBTNAng( ").append(firstAction);
      sb.append(", ").append(viewModeAction);
      sb.append(", ").append(a);
      sb.append(", ").append(obj).append(")\n");
      System.out.println(sb);
    }

    if (!this.wasUpdated()) {
      this.update();
      if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
        System.out.println("needed Update");
      }
    }

    final double viewWidth = (viewModeAction == null) ? this.getWorld()
        .getViewWidth() : viewModeAction.getViewWidth();
    final Player p = this.getWorld().getSelfRef().cloned();
    p.predictThisAfterAction(firstAction);
    // best view angle

    if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
      System.out.print("call bestViewDirAbs(");
      System.out.print(p.getBodyDir());
      System.out.print(", ");
      System.out.print(viewWidth);
      System.out.print(", ");
      System.out.print(a);
      System.out.print(", ");
      System.out.print(obj);
      System.out.println(")");
    }

    final double angleAbs = this.bestViewDirAbs(p.getBodyDir(), viewWidth, a,
        obj);

    if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
      System.out.print("resulting angle: ");
      System.out.println(angleAbs);
    }

    // some corrections....
    double angleToBody = Vektor.normalize(angleAbs - p.getBodyDir());

    // this should never happen
    if ((angleToBody < SConf.getInstance().MINNECKANG - this.getWorld()
        .getViewSliceWidth()) || (angleToBody > SConf.getInstance().MAXNECKANG + this
        .getWorld().getViewSliceWidth())) {
      System.out
          .println("NeckRotator breaks neck: C" + this.getWorld().getCycle() + " P" + this
              .getWorld().getNumber() + "State:" + a + " result. Neckangle=" + angleToBody + " abs.Angle=" + angleAbs + " BD:" + p
              .getBodyDir() + " ND:" + p.getNeckAngle());
      this.showValues(a, 0);
    }

    // as the NeckRotator is discret, it might sometimes return to high
    // values
    // max error is neckRotator.getSliceWidth() (11.5� by default)
    if (angleToBody < SConf.getInstance().MINNECKANG) {
      // + 0.001 for avoiding 180� turn in wrong direction
      angleToBody = SConf.getInstance().MINNECKANG + 0.001;
    }
    if (angleToBody > SConf.getInstance().MAXNECKANG) {
      // - 0.001 for avoiding 180� turn in wrong direction
      angleToBody = SConf.getInstance().MAXNECKANG - 0.001;
    }

    if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
      System.out.print("resulting angle: ");
      System.out.println(Vektor.normalize(angleToBody - p.getNeckAngle()));
      System.out.println("==================================================");
    }
    return Vektor.normalize(angleToBody - p.getNeckAngle());
  }

  /**
   * The best angle to turn the neck.
   * 
   * @param firstAction -
   *          the choosen main action
   * @param viewModeAction -
   *          the choosen view mode action
   * @param a -
   *          calling action (state)
   * @param preferred -
   *          preferred (and unwanted) objects to see
   * 
   * @return angle to turn neck (rel to headAngle)
   * 
   */
  public double getBestTurnNeckAngle(final Action firstAction,
      final ChangeViewModeAction viewModeAction,
      final STATES a,
      final double[] preferred) {

    if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
      final StringBuffer sb = new StringBuffer();
      sb.append("C: ").append(this.getWorld().getCycle());
      sb.append(" BS: ").append(this.getWorld().getBodyCycle());
      sb.append("\ncall Prophet.getBTNAng( ").append(firstAction);
      sb.append(", ").append(viewModeAction);
      sb.append(", ").append(a);
      sb.append(", ").append(preferred).append(")\n");
      System.out.println(sb);
    }
    if (!this.wasUpdated()) {
      this.update();
      if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
        System.out.println("needed Update");
      }
    }

    final double viewWidth = (viewModeAction == null) ? this.getWorld()
        .getViewWidth() : viewModeAction.getViewWidth();
    final Player p = this.getWorld().getSelfRef().cloned();
    p.predictThisAfterAction(firstAction);
    // best view angle

    if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
      System.out.print("call bestViewDirAbs(");
      System.out.print(p.getBodyDir());
      System.out.print(", ");
      System.out.print(viewWidth);
      System.out.print(", ");
      System.out.print(a);
      System.out.println(")");
    }

    final double angleAbs = this.bestViewDirAbs(p.getBodyDir(), viewWidth, a,
        preferred);

    if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
      System.out.print("resulting angle: ");
      System.out.println(angleAbs);
    }

    // some corrections....
    double angleToBody = Vektor.normalize(angleAbs - p.getBodyDir());

    // this should never happen
    if ((angleToBody < SConf.getInstance().MINNECKANG - this.getWorld()
        .getViewSliceWidth()) || (angleToBody > SConf.getInstance().MAXNECKANG + this
        .getWorld().getViewSliceWidth())) {
      System.out
          .println("NeckRotator breaks neck: C" + this.getWorld().getCycle() + " P" + this
              .getWorld().getNumber() + "State:" + a + " result. Neckangle=" + angleToBody + " abs.Angle=" + angleAbs + " BD:" + p
              .getBodyDir() + " ND:" + p.getNeckAngle());
      this.showValues(a, 0);
    }

    // as the NeckRotator is discret, it might sometimes return to high
    // values
    // max error is neckRotator.getSliceWidth() (11.5� by default)
    if (angleToBody < SConf.getInstance().MINNECKANG) {
      // + 0.001 for avoiding 180� turn in wrong direction
      angleToBody = SConf.getInstance().MINNECKANG + 0.001;
    }
    if (angleToBody > SConf.getInstance().MAXNECKANG) {
      // - 0.001 for avoiding 180� turn in wrong direction
      angleToBody = SConf.getInstance().MAXNECKANG - 0.001;
    }

    if ((this.getWorld().getNumber() == 2) && (this.getWorld().getCycle() > 0) && this.debugTN) {
      System.out.print("resulting angle: ");
      System.out.println(Vektor.normalize(angleToBody - p.getNeckAngle()));
      System.out.println("==================================================");
    }
    return Vektor.normalize(angleToBody - p.getNeckAngle());
  }
}
